
#include "gridmap.h"

namespace Group5 {

Gridmap::Gridmap() : start(0,0), output( "map.txt", fstream::in | fstream::out ),
  current_direction(NORTH), second_order_(third_order_), first_order_(second_order_) {
  start.set_start();
  current = &start;
  posmap[ start.get_position() ] = &start;
}

Gridmap::~Gridmap() {
  close_output();
}

Position* Gridmap::current_position() const {
  Position p = (*current).get_position();
  Position* np = new Position( p.x, p.y );
  return np;
}

GridRec* Gridmap::search( Grid* g1, Grid* g2 ) {

  while( !bfq.empty() ) bfq.pop();
  usedmap.clear();

  GridRec * gr = new GridRec( g1, NULL );

  bfq.push(gr);

  int i = 0;

  while( !bfq.empty() ) {

    GridRec& curr = *(bfq.front());
    bfq.pop();

    if ( *(curr.grid) == *g2 ) {
      //cout << "Found optimal path to: " << *((*gr).grid) << endl;
      //cout << "path length: " << curr.path_length( &curr ) << endl;
      //curr.print_path( &curr );
      //break;
      return &curr;
    }

    Grid * temp = (*(curr.grid)).get_grid(NORTH);
    if( temp != 0 && usedmap.count(*temp) == 0 ) {
      GridRec* grt = new GridRec( temp, &curr );
      bfq.push( grt );
      usedmap[*temp] = true;
    }

    temp = (*(curr.grid)).get_grid(SOUTH);
    if( temp != 0 && usedmap.count(*temp) == 0 ) {
      GridRec* grt = new GridRec( temp, &curr );
      bfq.push( grt );
      usedmap[*temp] = true;
    }

    temp = (*(curr.grid)).get_grid(EAST);
    if( temp != 0 && usedmap.count(*temp) == 0 ) {
      GridRec* grt = new GridRec( temp, &curr );
      bfq.push( grt );
      usedmap[*temp] = true;
    }

    temp = (*(curr.grid)).get_grid(WEST);
    if( temp != 0 && usedmap.count(*temp) == 0 ) {
      GridRec* grt = new GridRec( temp, &curr );
      bfq.push( grt );
      usedmap[*temp] = true;
    }
    ++i;
  }
  return NULL;
}

void Gridmap::go_to(Grid *g) {
  current_step_ = search(current, g);
}

  

const bool Gridmap::add_opening( int direction ) {

  Position p = (*current).get_position().adjacent_pos(direction);

  // Grid already exists
  if ( posmap.count( p ) > 0 ) {

    (*current).add_grid( direction, posmap[p] );

    return true;
  }
  else { //Grid does not exist

    Grid& g = (*current).add_grid( direction );
    posmap[p] = &g;

    return false;
  }
}

Grid* Gridmap::move( int direction ) {

  current_direction = direction;

  Position p = (*current).get_position().adjacent_pos(direction);

  // Grid already exists
  if ( posmap.count( p ) > 0 ) {

    (*current).leave();

    (*current).add_grid( direction, posmap[p] );
    current = posmap[p];

    (*current).enter();
    (*current).set_explored();

    //Log::Write( "current grid: (%d, %d)", (*current).get_x(), (*current).get_y()  );
    output << "current grid: (";
    output << (*current).get_x();
    output << ", ";
    output << (*current).get_y();
    output << ")\n";

    return current;
  }
  else { //Grid does not exist

    (*current).leave();

    Grid& g = (*current).add_grid( direction );
    posmap[p] = &g;
    current = &g;

    (*current).enter();
    (*current).set_explored();

    //Log::Write( "current grid: (%d, %d)", (*current).get_x(), (*current).get_y()  );
    output << "current grid: (";
    output << (*current).get_x();
    output << ", ";
    output << (*current).get_y();
    output << ")\n";

    return current;
  }

}

Order* Gridmap::get_order() {
  // Checking if we have a valid path to follow
  if (current_step_ == NULL || current_step_->father == NULL) {
    if (current_step_) {
      if(LOG_PATH_FOLLOWING) Log::Write("Mapping: Arrived!");
      delete current_step_;
      current_step_ = NULL;
    }
    // Default strategy
    if(LOG_PATH_FOLLOWING) Log::Write("Mapping: Default strategy");
    third_order_.direction = RIGHT;
    second_order_.direction = FORWARD;
    first_order_.direction = LEFT;
    return &first_order_;
  }

  if (current_step_->grid != current) {
    // Maybe we are at the next step
    if (current_step_->father->grid == current) {
      // Going to the next step
      if (LOG_PATH_FOLLOWING) Log::Write("Mapping: Going to next grid on the path");
      GridRec* tmp = current_step_;
      current_step_ = current_step_->father;
      delete tmp;
    }
    // Otherwise the robot should hopefully be going back to the right path
    // So let's wait
    else {
      if (LOG_PATH_FOLLOWING) Log::Write("Mapping: Robot lost the path, waiting for it to come back");
      first_order_.direction = FORWARD;
      second_order_.direction = BACKWARD;
      return &first_order_;
    }
  }

  // If we get here we have a valid path to follow
  GridRec* next_step = current_step_->father;
  if ( (current->north == next_step->grid && current_direction == NORTH)
    || (current->west == next_step->grid && current_direction == WEST)
    || (current->east == next_step->grid && current_direction == EAST)
    || (current->south == next_step->grid && current_direction == SOUTH) ) {
    // The robot must go forward
    if (LOG_PATH_FOLLOWING) Log::Write("Mapping: Robot should continue forward");
    first_order_.direction = FORWARD;
    second_order_.direction = BACKWARD;
  }
  else if ( (current->north == next_step->grid && current_direction == WEST)
    || (current->west == next_step->grid && current_direction == SOUTH)
    || (current->east == next_step->grid && current_direction == NORTH)
    || (current->south == next_step->grid && current_direction == EAST) ) {
    // The robot must go left
    if (LOG_PATH_FOLLOWING) Log::Write("Mapping: Robot should turn left");
    first_order_.direction = LEFT;
    second_order_.direction = FORWARD;
    third_order_.direction = BACKWARD;
  }
  else if ( (current->north == next_step->grid && current_direction == EAST)
    || (current->west == next_step->grid && current_direction == NORTH)
    || (current->east == next_step->grid && current_direction == SOUTH)
    || (current->south == next_step->grid && current_direction == WEST) ) {
    // The robot must go right
    if (LOG_PATH_FOLLOWING) Log::Write("Mapping: Robot should turn right");
    first_order_.direction = RIGHT;
    second_order_.direction = FORWARD;
    third_order_.direction = BACKWARD;
  }
  else if ( (current->north == next_step->grid && current_direction == SOUTH)
    || (current->west == next_step->grid && current_direction == EAST)
    || (current->east == next_step->grid && current_direction == WEST)
    || (current->south == next_step->grid && current_direction == NORTH) ) {
    // The robot must go backward (shouldn't happen)
    if (LOG_PATH_FOLLOWING) Log::Write("Mapping: Robot should uturn");
    first_order_.direction = BACKWARD;
  }
    return &first_order_;
}

};
